#!/usr/bin/env python
# coding:utf-8
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import String
import re
from tf.transformations import quaternion_from_euler

# 正则表达式用于匹配 "xxx,yyy,zzz" 格式
pattern = re.compile(r'^(-?\d+\.\d+),(-?\d+\.\d+),(-?\d+\.\d+)$')

# 定义一个全局变量来控制暂停状态
pause = False

def qrcode_callback(msg):
    global pause
    if pause:
        return

    data = msg.data
    match = pattern.match(data)
    if match:
        x, y, yaw = match.groups()
        rospy.loginfo("Detected coordinates: X={}, Y={}, Yaw={}".format(x, y, yaw))
        
        # 创建一个MoveBaseGoal对象并设置目标点
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = float(x)
        goal.target_pose.pose.position.y = float(y)
        
        # 将yaw转换为四元数
        quaternion = quaternion_from_euler(0, 0, float(yaw))
        goal.target_pose.pose.orientation.x = quaternion[0]
        goal.target_pose.pose.orientation.y = quaternion[1]
        goal.target_pose.pose.orientation.z = quaternion[2]
        goal.target_pose.pose.orientation.w = quaternion[3]

        # 发送目标点给move_base
        client.send_goal(goal)
        rospy.loginfo("Goal sent to move_base: X={}, Y={}, Yaw={}".format(x, y, yaw))

        # 发布指令到特定的topic
        command_pub.publish("Goal received and sent to move_base")

        # 设置暂停状态并启动定时器
        pause = True
        rospy.Timer(rospy.Duration(10), reset_pause, oneshot=True)
    else:
        rospy.loginfo("Data does not match the required format")

def reset_pause(event):
    global pause
    pause = False

def main():
    global client, command_pub
    rospy.init_node('send_goal_node', anonymous=True)
    
    # 创建一个SimpleActionClient，使用move_base的MoveBaseAction
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    
    # 创建一个Publisher用于发布指令
    command_pub = rospy.Publisher('command_topic', String, queue_size=10)
    
    rospy.Subscriber('QRcode', String, qrcode_callback)
    rospy.spin()

if __name__ == '__main__':
    main()